#include "stm32_robot/serial_driver.hpp"
#include <rclcpp/logging.hpp>
#include <stdexcept>
#include <fcntl.h>
#include <unistd.h>
#include <sys/stat.h>

LibSerial::BaudRate GetBaudRateEnum(int baud) {
    switch(baud) {
        case 1200: return LibSerial::BaudRate::BAUD_1200;
        case 2400: return LibSerial::BaudRate::BAUD_2400;
        case 4800: return LibSerial::BaudRate::BAUD_4800;
        case 9600: return LibSerial::BaudRate::BAUD_9600;
        case 19200: return LibSerial::BaudRate::BAUD_19200;
        case 38400: return LibSerial::BaudRate::BAUD_38400;
        case 57600: return LibSerial::BaudRate::BAUD_57600;
        case 115200: return LibSerial::BaudRate::BAUD_115200;
        case 230400: return LibSerial::BaudRate::BAUD_230400;
        case 460800: return LibSerial::BaudRate::BAUD_460800;
        case 921600: return LibSerial::BaudRate::BAUD_921600;
        default:
            throw std::invalid_argument("Unsupported baud rate: " + std::to_string(baud));
    }
}

void SerialDriver::Setup(const std::string& port, int baud) {
    std::lock_guard<std::mutex> lock(serial_mutex_);
    
    try {
        // 检查设备是否存在
        if (access(port.c_str(), F_OK) == -1) {
            RCLCPP_FATAL(rclcpp::get_logger("SerialDriver"), "Device not found: %s", port.c_str());
            throw std::runtime_error("Device not found");
        }
        
        // 检查设备权限
        if (access(port.c_str(), R_OK | W_OK) == -1) {
            RCLCPP_FATAL(rclcpp::get_logger("SerialDriver"), "Permission denied for: %s", port.c_str());
            RCLCPP_INFO(rclcpp::get_logger("SerialDriver"), "Try: sudo chmod a+rw %s", port.c_str());
            throw std::runtime_error("Permission denied");
        }
        
        // 确保关闭已打开的串口
        if (serial_port_.IsOpen()) {
            RCLCPP_INFO(rclcpp::get_logger("SerialDriver"), "Closing previously opened port");
            serial_port_.Close();
        }
        
        RCLCPP_INFO(rclcpp::get_logger("SerialDriver"), "Opening port: %s @ %d baud", port.c_str(), baud);
        
        // 打开串口
        serial_port_.Open(port);
        
        // 设置波特率
        serial_port_.SetBaudRate(GetBaudRateEnum(baud));
        
        // 其他设置
        serial_port_.SetCharacterSize(LibSerial::CharacterSize::CHAR_SIZE_8);
        serial_port_.SetFlowControl(LibSerial::FlowControl::FLOW_CONTROL_NONE);
        serial_port_.SetParity(LibSerial::Parity::PARITY_NONE);
        serial_port_.SetStopBits(LibSerial::StopBits::STOP_BITS_1);
        
        RCLCPP_INFO(rclcpp::get_logger("SerialDriver"), "Serial port initialized successfully");
    } 
    catch (const LibSerial::OpenFailed& e) {
        RCLCPP_FATAL(rclcpp::get_logger("SerialDriver"), "Port open failed: %s", e.what());
        throw;
    }
    catch (const std::invalid_argument& e) {
        RCLCPP_FATAL(rclcpp::get_logger("SerialDriver"), "Invalid argument: %s", e.what());
        throw;
    }
    catch (const std::exception& e) {
        RCLCPP_FATAL(rclcpp::get_logger("SerialDriver"), "Unexpected error: %s", e.what());
        throw;
    }
}

void SerialDriver::Write(const std::vector<uint8_t>& data) {
    std::lock_guard<std::mutex> lock(serial_mutex_);
    
    try {
        if (!serial_port_.IsOpen()) {
            RCLCPP_ERROR(rclcpp::get_logger("SerialDriver"), "Attempt to write to closed port");
            return;
        }
        
        serial_port_.Write(data);
        RCLCPP_DEBUG(rclcpp::get_logger("SerialDriver"), "Wrote %zu bytes to serial", data.size());
    } 
    catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("SerialDriver"), "Serial write error: %s", e.what());
    }
}



void SerialDriver::Close() {
    std::lock_guard<std::mutex> lock(serial_mutex_);
    
    try {
        if (serial_port_.IsOpen()) {
            RCLCPP_INFO(rclcpp::get_logger("SerialDriver"), "Closing serial port");
            serial_port_.Close();
            RCLCPP_INFO(rclcpp::get_logger("SerialDriver"), "Serial port closed");
        }
    } 
    catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("SerialDriver"), "Error closing port: %s", e.what());
    }
}

bool SerialDriver::IsOpen() const {
    std::lock_guard<std::mutex> lock(serial_mutex_);
    return serial_port_.IsOpen();
}


bool SerialDriver::ReadByte(uint8_t &byte, int timeout_ms) {
  std::lock_guard<std::mutex> lock(serial_mutex_);
  if (!serial_port_.IsOpen()) return false;
  
  try {
    // LibSerial的ReadByte函数带超时参数
    serial_port_.ReadByte(byte, timeout_ms);
    return true;
  } catch (const std::exception& e) {
    RCLCPP_ERROR(rclcpp::get_logger("SerialDriver"), "Read error: %s", e.what());
    return false;
  }
}